#include <boost\math\quaternion.hpp>
#include "DMath.h"
#include "../Tools/nvMath.h"

using namespace nv;

template<typename T>
class qtx: public boost::math::quaternion<T>
{
public:
	void GetRotationMatrix(matrix4<T> & mtx )
	{
		/*float q[4];
		q[0] = this->a;
		q[1] = this->b;
		q[2] = this->c;
		q[3] = this->d;

		DMath::LoadIdentity44(mtx._array);*/
		/*
		if(q[0] == 0.0f && q[1] == 0.0f && q[2] == 0.0f && q[3] == 0.0f)
		{
			return;
		}*/
		/*
		T wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

		//T s  = 2.0f/QuaternionNorm(q);  // 4 mul 3 add 1 div
		T s  = 2.0f/norm(*this);  // 4 mul 3 add 1 div

		x2 = q[0] * s;    y2 = q[1] * s;    z2 = q[2] * s;
		xx = q[0] * x2;   xy = q[0] * y2;   xz = q[0] * z2;
		yy = q[1] * y2;   yz = q[1] * z2;   zz = q[2] * z2;
		wx = q[3] * x2;   wy = q[3] * y2;   wz = q[3] * z2;

		mtx._array[0] = 1.0f - (yy + zz);
		mtx._array[1] = xy + wz;
		mtx._array[2] = xz - wy;
		mtx._array[4] = xy - wz;
		mtx._array[5] = 1.0f - (xx + zz);
		mtx._array[6] = yz + wx;
		mtx._array[8] = xz + wy;
		mtx._array[9] = yz - wx;
		mtx._array[10] = 1.0f - (xx + yy);
		*/
		//
		
		T s, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;

        T norm = a * a + b * b + c * c + d * d;

        s = ( norm == T(0.0)) ? T(0.0) : ( T(2.0) / norm );

        xs = a * s;
        ys = b * s;
        zs = c * s;

        wx = d * xs;
        wy = d * ys;
        wz = d * zs;

        xx = a * xs;
        xy = a * ys;
        xz = a * zs;

        yy = b * ys;
        yz = b * zs;
        zz = c * zs;

        mtx(0,0) = T( T(1.0) - ( yy + zz ));
        mtx(1,0) = T ( xy + wz );
        mtx(2,0) = T ( xz - wy );

        mtx(0,1) = T ( xy - wz );
        mtx(1,1) = T ( T(1.0) - ( xx + zz ));
        mtx(2,1) = T ( yz + wx );

        mtx(0,2) = T ( xz + wy );
        mtx(1,2) = T ( yz - wx );
        mtx(2,2) = T ( T(1.0) - ( xx + yy ));

        mtx(3,0) = mtx(3,1) = mtx(3,2) = mtx(0,3) = mtx(1,3) = mtx(2,3) = T(0.0);
        mtx(3,3) = T(1.0);
	}


	//======
	/*quaternion & */void FormAxisAngle( const vec3<T> &axis, T theta )
    {
        T sqnorm = square_norm(axis);

        if (sqnorm == T(0.0))
        {
            // axis too small.
            a = b = c = T(0.0);
            d = T(1.0);
        } 
        else 
        {
            theta *= T(0.5);
            T sin_theta = T(sin(theta));

            if ( sqnorm != T(1)) 
                sin_theta /= T(sqrt(sqnorm));
            a = sin_theta * axis[0];
            b = sin_theta * axis[1];
            c = sin_theta * axis[2];
            d = T(cos(theta));
        }
        /*return *this;*/
    }


	///

	qtx(T x, T y, T z, T w)
	{
		a = x;
		b = y;
		c = z;
		d = w;
	}

	qtx()
	{
		boost::math::quaternion<float>();
	}

	void SetValues(T x, T y, T z, T w)
	{
		a = x;
		b = y;
		c = z;
		d = w;
	}


	void mult_vec( const vec3<T> &src, vec3<T> &dst ) const
    {
		/*qtx<T> q, r, br;
		
		r.SetValues(0.0f, 0.0f, 0.0f, 1.0f);

		q.SetValues(src.x, src.y, src.z, T(1.0));
		br.SetValues(a, b, c, d);

		r *= *this;
		r *= q;
		r *= br;
		
		dst[0] = r.R_component_1();
		dst[1] = r.R_component_2();
		dst[2] = r.R_component_3();*/
		
        T v_coef = d * d - a * a - b * b - c * c;                     
        T u_coef = T(2.0) * (src[0] * a + src[1] * b + src[2] * c);  
        T c_coef = T(2.0) * d;                                       

        dst[0] = v_coef * src[0] + u_coef * a + c_coef * (b * src[2] - c * src[1]);
        dst[1] = v_coef * src[1] + u_coef * b + c_coef * (c * src[0] - a * src[2]);
        dst[2] = v_coef * src[2] + u_coef * c + c_coef * (a * src[1] - b * src[0]);
    }

	friend qtx<T> operator * ( const qtx<T> & lhs, const qtx<T> & rhs ) {	
        qtx r;
		
		r.SetValues(lhs.a, lhs.b, lhs.c, lhs.d);
        r *= rhs; 
        return r; 
    }
};

class boostCam
{
public:
	qtx<float> rotX;
	qtx<float> rotY;
	qtx<float> rotZ;

	float pitch;
	float yaw;

	vec3f location;

	boostCam()
	{
		rotX.SetValues(0.0f, 0.0f, 0.0f, 1.0f);
		rotY.SetValues(0.0f, 0.0f, 0.0f, 1.0f);
		rotZ.SetValues(0.0f, 0.0f, 0.0f, 1.0f);

		pitch = 0.0f;
		yaw = 0.0f;

		location.x = 0.0f;
		location.y = 0.0f;
		location.z = 0.0f;
	}

	void GetMatrix(matrix4f & mtx)
	{
		qtx<float> res;

		res = rotX * rotY * rotZ;

		/*res.SetValues(0.0f, 0.0f, 0.0f, 1.0f);
		res *= rotX;
		res *= rotY;
		res *= rotZ;*/

		res.GetRotationMatrix(mtx);
	}


	void Pitch(float angle)
	{
		vec3f vc;
		vc.x = 1.0f;
		vc.y = 0.0f;
		vc.z = 0.0f;

		pitch += angle;
		rotX.FormAxisAngle(vc, pitch);
	}

	void Yaw(float angle)
	{
		vec3f vc;
		vc.x = 0.0f;
		vc.y = 1.0f;
		vc.z = 0.0f;

		yaw += angle;
		rotY.FormAxisAngle(vc, yaw);
	}

	void Move(float move)
	{
		vec3f vc, vc2;

		vc.x = 0.0f;
		vc.y = 0.0f;
		vc.z = 1.0f;

		rotZ.mult_vec(vc, vc2);

		location.x += vc2.x * move;
		location.y += vc2.y * move;
		location.z += vc2.z * move;		
	}

	void Strafe(float move)
	{
		//
	}

};